/***************************************************************************
 创建者: 林逢达
 开始时间: 2016.11.28
 copyright: (C) 同川科技
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)

 ***************************************************************************
 *  @file jointdecouple.cpp
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#include "jointdecouple.h"
#include <qdebug.h>

//#define IF_DEBUG true
#define IF_DEBUG false

JointDecouple::JointDecouple(QVector<JointAttribute> *_robotAttribute, QVector<MotorData> *_motorFeedback):
    robotAttribute(_robotAttribute),
    motorFeedback(_motorFeedback)
{

    //setComponentVersion( "JointDecouple", "2016-12-9", "1.0.1" );

    jointNum = robotAttribute->size();
    position.resize(jointNum );
    vel.resize(jointNum);
    acc.resize(jointNum);
    torque.resize(jointNum);
    positionFeedback.resize(jointNum);
    velFeedback.resize(jointNum);
    torqueFeedback.resize(jointNum);
}

//void JointDecouple::jointDecouple(QVector<JointData> jointData)
//{
//    QVector<JointData> jointDataTemp;
//    QVector<MotorData> motorData;
//    motorData.resize(jointData.size());//初始化大小
//    jointDataTemp.resize(jointData.size());//初始化大小

//    //------------------------------去除关节耦合position\ velocity------------------------------
//    for(int i=0;i<jointData.size();i++)
//    {
//        //该轴为关节运动,且耦合轴也为关节

//      if( (*robotAttribute)[i].screwPitch==0
//              &&    (*robotAttribute)[         ((*robotAttribute)[i].coupleJoint)         ].screwPitch==0
//              )
//        {
//             //关节耦合补偿后的角度＝关节原来角度＋耦合关节的角度×耦合减速比
//            jointDataTemp[i].position=jointData[i].position + jointData[ (*robotAttribute)[i].coupleJoint-1 ].position* (*robotAttribute)[i].coupleReducer;
//            //关节耦合补偿后的速度＝关节原来速度＋耦合关节的速度×耦合减速比
//           jointDataTemp[i].speed=jointData[i].speed + jointData[ (*robotAttribute)[i].coupleJoint-1 ].speed* (*robotAttribute)[i].coupleReducer;

//        }

//        //该轴为直线运动,且耦合轴为关节
//       else    if( (*robotAttribute)[i].screwPitch !=0
//                      &&    (*robotAttribute)[         ((*robotAttribute)[i].coupleJoint)         ].screwPitch==0
//                      )
//        {
//            //直线耦合补偿后的长度＝直线原来长度＋（耦合关节的角度／３６０）×　螺距×耦合减速比
//           jointDataTemp[i].position=jointData[i].position +( jointData[ (*robotAttribute)[i].coupleJoint-1 ].position/360) * (*robotAttribute)[i].screwPitch *  (*robotAttribute)[i].coupleReducer;
//             //直线耦合补偿后的长度速度＝直线原来长度速度＋（耦合关节的角度速度／３６０）×　螺距×耦合减速比
//            jointDataTemp[i].speed=jointData[i].speed +( jointData[ (*robotAttribute)[i].coupleJoint-1 ].speed/360) * (*robotAttribute)[i].screwPitch *  (*robotAttribute)[i].coupleReducer;

//        }
//       else
//        {
//            //QDebug("关节解偶补偿错误，未支持补偿类型！");

//        }
//    }


//        //------------------------------　减速比和螺距　转换------------------------------
//        for(int i=0;i<jointData.size();i++)
//        {
//            if( (*robotAttribute)[i].screwPitch==0)//该轴为关节运动
//            {
//                //电机编码器值＝（关节角度／３６０）×减速比×２^编码器分辨率
//                 motorData[i].position=(jointDataTemp[i].position/360)* (*robotAttribute)[i].gearRatio*(2^ (*robotAttribute)[i].encoderResolution );
//                 //电机速度＝（关节角度速度／３６０）×减速比×２^编码器分辨率
//                  motorData[i].speed=(jointDataTemp[i].speed/360)* (*robotAttribute)[i].gearRatio*(2^ (*robotAttribute)[i].encoderResolution );

//            }
//           else  if( (*robotAttribute)[i].screwPitch!=0)//该轴为直线运动
//            {
//                //电机编码器值＝（关节距离／螺距）×减速比×２^编码器分辨率
//                 motorData[i].position=(jointDataTemp[i].position/ (*robotAttribute)[i].screwPitch ) *  (*robotAttribute)[i].gearRatio*(2^ (*robotAttribute)[i].encoderResolution );
//                 //电机速度＝（关节距离／螺距）×减速比×２^编码器分辨率
//                  motorData[i].speed=(jointDataTemp[i].speed/ (*robotAttribute)[i].screwPitch ) *  (*robotAttribute)[i].gearRatio*(2^ (*robotAttribute)[i].encoderResolution );
//            }


//        }

//          //------------------------------　电机数据存储进入缓存------------------------------
//        motorDataMutexMap.lock();
//        motorDataQueue.append(motorData);
//        motorDataMutexMap.unlock();
//}

int JointDecouple::decouple(const JntArray& jointsPosition, const JntArray& jointsVel, const JntArray& jointAcc)
{
    //-----------------------关节单位转换-----------------------
    for(int i=0; i<jointNum; i++)
    {
//        JointAttribute joint = (*robotAttribute)[i];
//        joint = *((*robotAttribute).begin());
//        joint = (*robotAttribute)[i];
        if( (*robotAttribute)[i].jointType==0 )//该轴为旋转关节
        {
            position(i) = jointsPosition(i) * 180.0 / M_PI;
            vel(i) = jointsVel(i) * 180.0 / M_PI;
            acc(i) = jointAcc(i) * 180.0 / M_PI;
        }
        else//该轴为移动关节
        {
            position(i)  = jointsPosition(i) * 1000.0;
            vel(i) = jointsVel(i) * 1000.0;
            acc(i) = jointAcc(i) * 1000.0;
        }
    }

    //-----------------------去除关节耦合position\ velocity \ acceleration-------------------------
    for(int i=0; i<jointNum; i++)
    {
        if((*robotAttribute)[i].coupleFlag)//该轴受其他轴耦合作用
        {
            if(IF_DEBUG)
            {
                qDebug() << " JointDecouple::decouple====关节解耦进行中";
            }
            int coupleNum = (*robotAttribute)[i].coupleJoint;
            //assert(coupleNum<i);
            if( ( 0==(*robotAttribute)[i].jointType) &&
                    ( 0==(*robotAttribute)[coupleNum].jointType ) )//该轴为关节运动,且耦合轴也为关节
            {
                //关节耦合补偿后的角度＝关节原来角度＋耦合关节的角度×耦合减速比
                position(i) +=position(coupleNum) * (*robotAttribute)[i].coupleReducer;
                //关节耦合补偿后的速度＝关节原来速度＋耦合关节的速度×耦合减速比
                vel(i) += vel(coupleNum) * (*robotAttribute)[i].coupleReducer;
                //关节耦合补偿后的加速度＝关节原来加速度＋耦合关节的加速度×耦合减速比
                acc(i) += acc(coupleNum) * (*robotAttribute)[i].coupleReducer;
            }
            else if( ( 1==(*robotAttribute)[i].jointType ) &&
                     ( 0==(*robotAttribute)[coupleNum].jointType ) )//该轴为直线运动,且耦合轴为关节
            {
                //直线耦合补偿后的长度＝直线原来长度＋（耦合关节的角度／360）×　螺距×耦合减速比
                position(i) += position(coupleNum) *  (*robotAttribute)[i].coupleReducer / 360.0 *(*robotAttribute)[i].screwPitch;
                //直线耦合补偿后的速度＝直线原来速度＋（耦合关节的角度速度／360）×　螺距×耦合减速比
                vel(i)+= vel(coupleNum) *  (*robotAttribute)[i].coupleReducer / 360.0 * (*robotAttribute)[i].screwPitch;
                //直线耦合补偿后的加速度＝直线原来加速度＋（耦合关节的角度加速度／360）×　螺距×耦合减速比
                acc(i)+= acc(coupleNum) *  (*robotAttribute)[i].coupleReducer / 360.0 * (*robotAttribute)[i].screwPitch;
            }
            else
            {
                return -1;//不支持解耦类型
            }
        }// end if((*robotAttribute)[i].coupleFlag)
    }//end for(int i=0;i<joints.rows();i++)
    return 1;
}

int JointDecouple::reverseDecouple()
{
    //-------单位换算---------
    for(int i=0; i<jointNum;i++)
    {
        if( 0 == (*robotAttribute)[i].jointType )//旋转关节
        {
            //关节角度 = 2pi * 电机编码器值/电机每圈的脉冲数/减速比
            positionFeedback(i) = 2.0 * M_PI / (*robotAttribute)[i].gearRatio * (*motorFeedback)[i].position / (*robotAttribute)[i].pulsePerRound;
            //关节角速度 = 2pi * 电机速度/电机每圈的脉冲数/减速比
            velFeedback(i) = 2.0 * M_PI / (*robotAttribute)[i].gearRatio * (*motorFeedback)[i].speed / (*robotAttribute)[i].pulsePerRound;
        }
        else//移动关节
        {
            //关节长度 = 螺矩 * 电机编码器值/电机每圈的脉冲数/减速比
            positionFeedback(i) = (*robotAttribute)[i].screwPitch / 1000.0 / (*robotAttribute)[i].gearRatio * (*motorFeedback)[i].position / (*robotAttribute)[i].pulsePerRound;
            //关节速度 = 螺矩 * 电机速度/电机每圈的脉冲数/减速比
            velFeedback(i) = (*robotAttribute)[i].screwPitch / 1000.0 / (*robotAttribute)[i].gearRatio * (*motorFeedback)[i].speed / (*robotAttribute)[i].pulsePerRound;
        }
    }

    //-------反解耦---------
    for(int i=jointNum-1; i>=0; i--)
    {
        if((*robotAttribute)[i].coupleFlag)//该轴受其他轴耦合作用
        {
            if(IF_DEBUG)
            {
                qDebug() << "JointDecouple::reverseDecouple====关节反解耦"<< i;
            }
            int coupleNum = (*robotAttribute)[i].coupleJoint;
            //assert(coupleNum<i);
            if( (0==(*robotAttribute)[i].jointType) &&
                    (0==(*robotAttribute)[coupleNum].jointType) )//该轴为关节运动,且耦合轴也为关节
            {
                //关节原来角度 = 关节耦合补偿后的角度 - 耦合关节的角度×耦合减速比
                positionFeedback(i) -= positionFeedback(coupleNum) * (*robotAttribute)[i].coupleReducer;
                //关节原来速度 = 关节耦合补偿后的速度 - 耦合关节的速度×耦合减速比
                velFeedback(i) -= velFeedback(coupleNum) * (*robotAttribute)[i].coupleReducer;
            }
            else if( (1==(*robotAttribute)[i].jointType) &&
                     (0==(*robotAttribute)[coupleNum].jointType) )//该轴为直线运动,且耦合轴为关节
            {
//                //关节原来长度 = 关节耦合补偿后的长度 -（耦合关节的角度／2pi）× 螺距×耦合减速比
//                positionFeedback(i) -= positionFeedback(coupleNum) * (*robotAttribute)[i].screwPitch / 2.0 / M_PI *  (*robotAttribute)[i].coupleReducer;
//                //关节原来速度 = 关节耦合补偿后的速度 -（耦合关节的角度速度／2pi）×　螺距×耦合减速比
//                velFeedback(i) -= velFeedback(coupleNum) *  (*robotAttribute)[i].coupleReducer / 2.0 / M_PI * (*robotAttribute)[i].screwPitch;

                //关节原来长度 = 关节耦合补偿后的长度 -（耦合关节的角度／2pi）× 螺距×耦合减速比
                positionFeedback(i) -= positionFeedback(coupleNum) * (*robotAttribute)[i].screwPitch / 2.0 / M_PI *  (*robotAttribute)[i].coupleReducer / 1000.0;
                //关节原来速度 = 关节耦合补偿后的速度 -（耦合关节的角度速度／2pi）×　螺距×耦合减速比
                velFeedback(i) -= velFeedback(coupleNum) *  (*robotAttribute)[i].coupleReducer / 2.0 / M_PI * (*robotAttribute)[i].screwPitch / 1000.0;
            }
            else
            {
                return -1;//不支持类型
            }
        }
    }
}
